#include "soft_i2c.h"


// 内联函数用于GPIO操作
static inline void soft_i2c_scl_high(soft_i2c_handle_t *handle) {
    HAL_GPIO_WritePin(handle->config.scl_port, handle->config.scl_pin, GPIO_PIN_SET);
}

static inline void soft_i2c_scl_low(soft_i2c_handle_t *handle) {
    HAL_GPIO_WritePin(handle->config.scl_port, handle->config.scl_pin, GPIO_PIN_RESET);
}

static inline void soft_i2c_sda_high(soft_i2c_handle_t *handle) {
    HAL_GPIO_WritePin(handle->config.sda_port, handle->config.sda_pin, GPIO_PIN_SET);
}

static inline void soft_i2c_sda_low(soft_i2c_handle_t *handle) {
    HAL_GPIO_WritePin(handle->config.sda_port, handle->config.sda_pin, GPIO_PIN_RESET);
}

static inline uint8_t soft_i2c_sda_read(soft_i2c_handle_t *handle) {
    return HAL_GPIO_ReadPin(handle->config.sda_port, handle->config.sda_pin);
}

// 微秒延迟函数（使用DWT或SysTick）
void soft_i2c_delay_us(uint32_t us) {
    // 使用DWT计数器（如果可用）
    #ifdef DWT_CYCCNT
    uint32_t cycles = (SystemCoreClock / 1000000) * us;
    uint32_t start = DWT->CYCCNT;
    while ((DWT->CYCCNT - start) < cycles);
    #else
    // 使用HAL延迟（不太精确但可用）
    uint32_t start = HAL_GetTick();
    while ((HAL_GetTick() - start) < ((us + 500) / 1000));
    #endif
}

// I2C初始化
soft_i2c_status_t soft_i2c_init(soft_i2c_handle_t *handle) {
    if (handle == NULL) {
        return SOFT_I2C_INVALID_PARAM;
    }
    
    if (handle->initialized) {
        return SOFT_I2C_OK;
    }
    
    // 使能GPIO时钟
    if (handle->config.scl_port == GPIOA) __HAL_RCC_GPIOA_CLK_ENABLE();
    else if (handle->config.scl_port == GPIOB) __HAL_RCC_GPIOB_CLK_ENABLE();
    else if (handle->config.scl_port == GPIOC) __HAL_RCC_GPIOC_CLK_ENABLE();
    else if (handle->config.scl_port == GPIOD) __HAL_RCC_GPIOD_CLK_ENABLE();
    else if (handle->config.scl_port == GPIOE) __HAL_RCC_GPIOE_CLK_ENABLE();
    else if (handle->config.scl_port == GPIOF) __HAL_RCC_GPIOF_CLK_ENABLE();
    else if (handle->config.scl_port == GPIOG) __HAL_RCC_GPIOG_CLK_ENABLE();
    else if (handle->config.scl_port == GPIOH) __HAL_RCC_GPIOH_CLK_ENABLE();
    else if (handle->config.scl_port == GPIOI) __HAL_RCC_GPIOI_CLK_ENABLE();
    
    if (handle->config.sda_port == GPIOA) __HAL_RCC_GPIOA_CLK_ENABLE();
    else if (handle->config.sda_port == GPIOB) __HAL_RCC_GPIOB_CLK_ENABLE();
    else if (handle->config.sda_port == GPIOC) __HAL_RCC_GPIOC_CLK_ENABLE();
    else if (handle->config.sda_port == GPIOD) __HAL_RCC_GPIOD_CLK_ENABLE();
    else if (handle->config.sda_port == GPIOE) __HAL_RCC_GPIOE_CLK_ENABLE();
    else if (handle->config.sda_port == GPIOF) __HAL_RCC_GPIOF_CLK_ENABLE();
    else if (handle->config.sda_port == GPIOG) __HAL_RCC_GPIOG_CLK_ENABLE();
    else if (handle->config.sda_port == GPIOH) __HAL_RCC_GPIOH_CLK_ENABLE();
    else if (handle->config.sda_port == GPIOI) __HAL_RCC_GPIOI_CLK_ENABLE();
    
    // 配置GPIO为开漏输出
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    
    GPIO_InitStruct.Pin = handle->config.scl_pin;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(handle->config.scl_port, &GPIO_InitStruct);
    
    GPIO_InitStruct.Pin = handle->config.sda_pin;
    HAL_GPIO_Init(handle->config.sda_port, &GPIO_InitStruct);
    
    // 初始状态：SCL和SDA都为高
    soft_i2c_scl_high(handle);
    soft_i2c_sda_high(handle);
    
    // 设置时钟延迟
    soft_i2c_set_speed(handle, handle->config.speed_mode, handle->config.clock_delay_us);
    
    // 配置DMA（如果启用）
    if (handle->config.use_dma) {
        if (handle->config.dma_stream == DMA1_Stream0) __HAL_RCC_DMA1_CLK_ENABLE();
        else if (handle->config.dma_stream == DMA2_Stream0) __HAL_RCC_DMA2_CLK_ENABLE();
        
        handle->hdma.Instance = handle->config.dma_stream;
        handle->hdma.Init.Channel = handle->config.dma_channel;
        handle->hdma.Init.Direction = DMA_MEMORY_TO_MEMORY;
        handle->hdma.Init.PeriphInc = DMA_PINC_ENABLE;
        handle->hdma.Init.MemInc = DMA_MINC_ENABLE;
        handle->hdma.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
        handle->hdma.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
        handle->hdma.Init.Mode = DMA_NORMAL;
        handle->hdma.Init.Priority = DMA_PRIORITY_HIGH;
        handle->hdma.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
        
        if (HAL_DMA_Init(&handle->hdma) != HAL_OK) {
            return SOFT_I2C_ERROR;
        }
    }
    
    // 创建总线互斥锁
    handle->bus_mutex = xSemaphoreCreateMutex();
    if (handle->bus_mutex == NULL) {
        if (handle->config.use_dma) {
            HAL_DMA_DeInit(&handle->hdma);
        }
        return SOFT_I2C_ERROR;
    }
    
    handle->initialized = true;
    handle->bus_acquired = false;
    
    return SOFT_I2C_OK;
}

// 设置速度
soft_i2c_status_t soft_i2c_set_speed(soft_i2c_handle_t *handle, soft_i2c_speed_mode_t speed_mode, uint32_t custom_delay_us) {
    if (!handle->initialized) {
        return SOFT_I2C_ERROR;
    }
    
    switch (speed_mode) {
        case SOFT_I2C_STANDARD_MODE:   // 100kHz
            handle->clock_delay = 5;   // 5us delay for 100kHz
            break;
        case SOFT_I2C_FAST_MODE:       // 400kHz
            handle->clock_delay = 2;   // 2.5us delay for 400kHz
            break;
        case SOFT_I2C_FAST_MODE_PLUS:  // 1MHz
            handle->clock_delay = 1;   // 1us delay for 1MHz
            break;
        case SOFT_I2C_CUSTOM_MODE:     // 自定义速度
            handle->clock_delay = custom_delay_us;
            break;
        default:
            return SOFT_I2C_INVALID_PARAM;
    }
    
    handle->config.speed_mode = speed_mode;
    handle->config.clock_delay_us = custom_delay_us;
    
    return SOFT_I2C_OK;
}

// I2C起始条件
static soft_i2c_status_t soft_i2c_start(soft_i2c_handle_t *handle) {
    soft_i2c_sda_high(handle);
    soft_i2c_scl_high(handle);
    soft_i2c_delay_us(handle->clock_delay);
    
    soft_i2c_sda_low(handle);
    soft_i2c_delay_us(handle->clock_delay);
    
    soft_i2c_scl_low(handle);
    soft_i2c_delay_us(handle->clock_delay);
    
    return SOFT_I2C_OK;
}

// I2C停止条件
static soft_i2c_status_t soft_i2c_stop(soft_i2c_handle_t *handle) {
    soft_i2c_sda_low(handle);
    soft_i2c_scl_high(handle);
    soft_i2c_delay_us(handle->clock_delay);
    
    soft_i2c_sda_high(handle);
    soft_i2c_delay_us(handle->clock_delay);
    
    return SOFT_I2C_OK;
}

// I2C发送应答
static void soft_i2c_send_ack(soft_i2c_handle_t *handle) {
    soft_i2c_sda_low(handle);
    soft_i2c_scl_high(handle);
    soft_i2c_delay_us(handle->clock_delay);
    soft_i2c_scl_low(handle);
    soft_i2c_delay_us(handle->clock_delay);
}

// I2C发送非应答
static void soft_i2c_send_nack(soft_i2c_handle_t *handle) {
    soft_i2c_sda_high(handle);
    soft_i2c_scl_high(handle);
    soft_i2c_delay_us(handle->clock_delay);
    soft_i2c_scl_low(handle);
    soft_i2c_delay_us(handle->clock_delay);
}

// I2C等待应答
static soft_i2c_status_t soft_i2c_wait_ack(soft_i2c_handle_t *handle) {
    soft_i2c_sda_high(handle); // 释放SDA
    soft_i2c_scl_high(handle);
    soft_i2c_delay_us(handle->clock_delay);
    
    // 配置SDA为输入
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = handle->config.sda_pin;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(handle->config.sda_port, &GPIO_InitStruct);
    
    // 检查ACK
    uint32_t timeout = 1000; // 超时计数器
    while (soft_i2c_sda_read(handle) && timeout--) {
        soft_i2c_delay_us(1);
    }
    
    // 恢复SDA为输出
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
    HAL_GPIO_Init(handle->config.sda_port, &GPIO_InitStruct);
    
    soft_i2c_scl_low(handle);
    soft_i2c_delay_us(handle->clock_delay);
    
    if (timeout == 0) {
        return SOFT_I2C_NACK;
    }
    
    return SOFT_I2C_OK;
}

// I2C发送一个字节
static soft_i2c_status_t soft_i2c_send_byte(soft_i2c_handle_t *handle, uint8_t data) {
    for (int8_t i = 7; i >= 0; i--) {
        if (data & (1 << i)) {
            soft_i2c_sda_high(handle);
        } else {
            soft_i2c_sda_low(handle);
        }
        
        soft_i2c_scl_high(handle);
        soft_i2c_delay_us(handle->clock_delay);
        soft_i2c_scl_low(handle);
        soft_i2c_delay_us(handle->clock_delay);
    }
    
    return soft_i2c_wait_ack(handle);
}

// I2C接收一个字节
static uint8_t soft_i2c_receive_byte(soft_i2c_handle_t *handle, bool ack) {
    uint8_t data = 0;
    
    // 配置SDA为输入
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = handle->config.sda_pin;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(handle->config.sda_port, &GPIO_InitStruct);
    
    for (int8_t i = 7; i >= 0; i--) {
        soft_i2c_scl_high(handle);
        soft_i2c_delay_us(handle->clock_delay);
        
        if (soft_i2c_sda_read(handle)) {
            data |= (1 << i);
        }
        
        soft_i2c_scl_low(handle);
        soft_i2c_delay_us(handle->clock_delay);
    }
    
    // 恢复SDA为输出
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
    HAL_GPIO_Init(handle->config.sda_port, &GPIO_InitStruct);
    
    if (ack) {
        soft_i2c_send_ack(handle);
    } else {
        soft_i2c_send_nack(handle);
    }
    
    return data;
}

// 获取总线控制权
soft_i2c_status_t soft_i2c_bus_acquire(soft_i2c_handle_t *handle, uint32_t timeout) {
    if (!handle->initialized) {
        return SOFT_I2C_ERROR;
    }
    
    if (xSemaphoreTake(handle->bus_mutex, pdMS_TO_TICKS(timeout)) != pdTRUE) {
        return SOFT_I2C_TIMEOUT;
    }
    
    handle->bus_acquired = true;
    return SOFT_I2C_OK;
}

// 释放总线控制权
soft_i2c_status_t soft_i2c_bus_release(soft_i2c_handle_t *handle) {
    if (!handle->initialized || !handle->bus_acquired) {
        return SOFT_I2C_ERROR;
    }
    
    xSemaphoreGive(handle->bus_mutex);
    handle->bus_acquired = false;
    return SOFT_I2C_OK;
}

// 写数据
soft_i2c_status_t soft_i2c_write(soft_i2c_handle_t *handle, uint8_t dev_addr, uint8_t *data, uint16_t len, uint32_t timeout) {
    if (!handle->initialized || data == NULL || len == 0) {
        return SOFT_I2C_INVALID_PARAM;
    }
    
    if (!handle->bus_acquired) {
        if (soft_i2c_bus_acquire(handle, timeout) != SOFT_I2C_OK) {
            return SOFT_I2C_TIMEOUT;
        }
    }
    
    soft_i2c_status_t status = SOFT_I2C_OK;
    
    // 发送起始条件
    if (soft_i2c_start(handle) != SOFT_I2C_OK) {
        status = SOFT_I2C_ERROR;
        goto cleanup;
    }
    
    // 发送设备地址（写模式）
    if (soft_i2c_send_byte(handle, dev_addr << 1) != SOFT_I2C_OK) {
        status = SOFT_I2C_NACK;
        goto cleanup;
    }
    
    // 发送数据
    for (uint16_t i = 0; i < len; i++) {
        if (soft_i2c_send_byte(handle, data[i]) != SOFT_I2C_OK) {
            status = SOFT_I2C_NACK;
            break;
        }
    }
    
cleanup:
    // 发送停止条件
    soft_i2c_stop(handle);
    
    if (!handle->bus_acquired) {
        soft_i2c_bus_release(handle);
    }
    
    return status;
}

// 读数据
soft_i2c_status_t soft_i2c_read(soft_i2c_handle_t *handle, uint8_t dev_addr, uint8_t *data, uint16_t len, uint32_t timeout) {
    if (!handle->initialized || data == NULL || len == 0) {
        return SOFT_I2C_INVALID_PARAM;
    }
    
    if (!handle->bus_acquired) {
        if (soft_i2c_bus_acquire(handle, timeout) != SOFT_I2C_OK) {
            return SOFT_I2C_TIMEOUT;
        }
    }
    
    soft_i2c_status_t status = SOFT_I2C_OK;
    
    // 发送起始条件
    if (soft_i2c_start(handle) != SOFT_I2C_OK) {
        status = SOFT_I2C_ERROR;
        goto cleanup;
    }
    
    // 发送设备地址（读模式）
    if (soft_i2c_send_byte(handle, (dev_addr << 1) | 0x01) != SOFT_I2C_OK) {
        status = SOFT_I2C_NACK;
        goto cleanup;
    }
    
    // 接收数据
    for (uint16_t i = 0; i < len; i++) {
        data[i] = soft_i2c_receive_byte(handle, i < (len - 1)); // 最后一个字节发送NACK
    }
    
cleanup:
    // 发送停止条件
    soft_i2c_stop(handle);
    
    if (!handle->bus_acquired) {
        soft_i2c_bus_release(handle);
    }
    
    return status;
}

// 写寄存器
soft_i2c_status_t soft_i2c_write_register(soft_i2c_handle_t *handle, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len, uint32_t timeout) {
    if (!handle->initialized || data == NULL || len == 0) {
        return SOFT_I2C_INVALID_PARAM;
    }
    
    if (!handle->bus_acquired) {
        if (soft_i2c_bus_acquire(handle, timeout) != SOFT_I2C_OK) {
            return SOFT_I2C_TIMEOUT;
        }
    }
    
    soft_i2c_status_t status = SOFT_I2C_OK;
    
    // 发送起始条件
    if (soft_i2c_start(handle) != SOFT_I2C_OK) {
        status = SOFT_I2C_ERROR;
        goto cleanup;
    }
    
    // 发送设备地址（写模式）
    if (soft_i2c_send_byte(handle, dev_addr << 1) != SOFT_I2C_OK) {
        status = SOFT_I2C_NACK;
        goto cleanup;
    }
    
    // 发送寄存器地址
    if (soft_i2c_send_byte(handle, reg_addr) != SOFT_I2C_OK) {
        status = SOFT_I2C_NACK;
        goto cleanup;
    }
    
    // 发送数据
    for (uint16_t i = 0; i < len; i++) {
        if (soft_i2c_send_byte(handle, data[i]) != SOFT_I2C_OK) {
            status = SOFT_I2C_NACK;
            break;
        }
    }
    
cleanup:
    // 发送停止条件
    soft_i2c_stop(handle);
    
    if (!handle->bus_acquired) {
        soft_i2c_bus_release(handle);
    }
    
    return status;
}

// 读寄存器
soft_i2c_status_t soft_i2c_read_register(soft_i2c_handle_t *handle, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len, uint32_t timeout) {
    if (!handle->initialized || data == NULL || len == 0) {
        return SOFT_I2C_INVALID_PARAM;
    }
    
    if (!handle->bus_acquired) {
        if (soft_i2c_bus_acquire(handle, timeout) != SOFT_I2C_OK) {
            return SOFT_I2C_TIMEOUT;
        }
    }
    
    soft_i2c_status_t status = SOFT_I2C_OK;
    
    // 发送起始条件
    if (soft_i2c_start(handle) != SOFT_I2C_OK) {
        status = SOFT_I2C_ERROR;
        goto cleanup;
    }
    
    // 发送设备地址（写模式）- 发送寄存器地址
    if (soft_i2c_send_byte(handle, dev_addr << 1) != SOFT_I2C_OK) {
        status = SOFT_I2C_NACK;
        goto cleanup;
    }
    
    // 发送寄存器地址
    if (soft_i2c_send_byte(handle, reg_addr) != SOFT_I2C_OK) {
        status = SOFT_I2C_NACK;
        goto cleanup;
    }
    
    // 重新起始条件
    if (soft_i2c_start(handle) != SOFT_I2C_OK) {
        status = SOFT_I2C_ERROR;
        goto cleanup;
    }
    
    // 发送设备地址（读模式）
    if (soft_i2c_send_byte(handle, (dev_addr << 1) | 0x01) != SOFT_I2C_OK) {
        status = SOFT_I2C_NACK;
        goto cleanup;
    }
    
    // 接收数据
    for (uint16_t i = 0; i < len; i++) {
        data[i] = soft_i2c_receive_byte(handle, i < (len - 1)); // 最后一个字节发送NACK
    }
    
cleanup:
    // 发送停止条件
    soft_i2c_stop(handle);
    
    if (!handle->bus_acquired) {
        soft_i2c_bus_release(handle);
    }
    
    return status;
}

// 检查设备是否就绪
soft_i2c_status_t soft_i2c_is_device_ready(soft_i2c_handle_t *handle, uint8_t dev_addr, uint32_t timeout) {
    if (!handle->initialized) {
        return SOFT_I2C_ERROR;
    }
    
    if (soft_i2c_bus_acquire(handle, timeout) != SOFT_I2C_OK) {
        return SOFT_I2C_TIMEOUT;
    }
    
    soft_i2c_status_t status = SOFT_I2C_OK;
    
    // 发送起始条件
    if (soft_i2c_start(handle) != SOFT_I2C_OK) {
        status = SOFT_I2C_ERROR;
        goto cleanup;
    }
    
    // 发送设备地址（写模式）
    if (soft_i2c_send_byte(handle, dev_addr << 1) != SOFT_I2C_OK) {
        status = SOFT_I2C_NACK;
    }
    
cleanup:
    // 发送停止条件
    soft_i2c_stop(handle);
    soft_i2c_bus_release(handle);
    
    return status;
}

// 单字节操作函数（简化版）
soft_i2c_status_t soft_i2c_write_byte(soft_i2c_handle_t *handle, uint8_t dev_addr, uint8_t data, uint32_t timeout) {
    return soft_i2c_write(handle, dev_addr, &data, 1, timeout);
}

soft_i2c_status_t soft_i2c_read_byte(soft_i2c_handle_t *handle, uint8_t dev_addr, uint8_t *data, uint32_t timeout) {
    return soft_i2c_read(handle, dev_addr, data, 1, timeout);
}

soft_i2c_status_t soft_i2c_write_reg_byte(soft_i2c_handle_t *handle, uint8_t dev_addr, uint8_t reg_addr, uint8_t data, uint32_t timeout) {
    return soft_i2c_write_register(handle, dev_addr, reg_addr, &data, 1, timeout);
}

soft_i2c_status_t soft_i2c_read_reg_byte(soft_i2c_handle_t *handle, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint32_t timeout) {
    return soft_i2c_read_register(handle, dev_addr, reg_addr, data, 1, timeout);
}

// I2C反初始化
soft_i2c_status_t soft_i2c_deinit(soft_i2c_handle_t *handle) {
    if (!handle->initialized) {
        return SOFT_I2C_OK;
    }
    
    if (handle->config.use_dma) {
        HAL_DMA_DeInit(&handle->hdma);
    }
    
    vSemaphoreDelete(handle->bus_mutex);
    
    // 恢复GPIO状态
    HAL_GPIO_DeInit(handle->config.scl_port, handle->config.scl_pin);
    HAL_GPIO_DeInit(handle->config.sda_port, handle->config.sda_pin);
    
    handle->initialized = false;
    handle->bus_acquired = false;
    
    return SOFT_I2C_OK;
}

// 获取默认配置
void soft_i2c_get_default_config(soft_i2c_config_t *config) {
    config->scl_port = GPIOB;
    config->scl_pin = GPIO_PIN_6;
    config->sda_port = GPIOB;
    config->sda_pin = GPIO_PIN_7;
    config->speed_mode = SOFT_I2C_STANDARD_MODE;
    config->clock_delay_us = 5;
    config->use_dma = false;
    config->dma_stream = DMA1_Stream0;
    config->dma_channel = DMA_CHANNEL_0;
    config->timeout_ms = 1000;
}
